// PoseableObject.h - An object which has both position and rotation
//
// Nick Kitten
// 2009

#include "PoseableObject.h"

///////////////////////////////////////////////////////
// constructor
PoseableObject::PoseableObject()
{
}

PoseableObject::PoseableObject( int id, float x, float y, float z, float rx, float ry, float rz )
: Object( id, x, y, z ), rotation( rx, ry, rz )
{
}

///////////////////////////////////////////////////////
// destructor
PoseableObject::~PoseableObject()
{
}

///////////////////////////////////////////////////////
// return a copy of the object's orientation
Triple 
PoseableObject::getRotation() const
{
	return this->rotation;
}

///////////////////////////////////////////////////////
// return the x coordinate of the rotation
float 
PoseableObject::getRotX() const
{
	return this->rotation.x;
}

///////////////////////////////////////////////////////
// return the y coordinate of the rotation
float 
PoseableObject::getRotY() const
{
	return this->rotation.y;
}

///////////////////////////////////////////////////////
// return the z coordinate of the rotation
float 
PoseableObject::getRotZ() const
{
	return this->rotation.z;
}

///////////////////////////////////////////////////////
// set the object's current orientation
void 
PoseableObject::setRotation(float x, float y, float z)
{
	// Children
	if( ! rotationChildren.empty() )
	{
		// Move children by the same amount this object is being moved
		// from its previous location.
		float dx, dy, dz;
		dx = x - this->getRotX();
		dy = y - this->getRotY();
		dz = z - this->getRotZ();

		list<RotationConstraint>::iterator child = rotationChildren.begin();
		Triple newRot;
		Triple oldPos, newPos;

		while( child != rotationChildren.end() )
		{
			newRot = child->obj->getRotation();
			oldPos = getPosition();
			newPos = oldPos;
			float distance = getDistance( oldPos, Triple(child->obj->getPosX(), oldPos.y, child->obj->getPosZ() ) );
			
			// X axis constraint exists
			if( child->axes.find( 'x' ) != child->axes.end() )
			{
				newRot.x = dbWrapValue( newRot.x + dx );
				newPos.y += dbNewYValue( oldPos.y, newRot.x, distance ) - oldPos.y;
				newPos.z += dbNewZValue( oldPos.z, newRot.x, distance ) - oldPos.z;
				newPos.x += child->obj->getPosX() - oldPos.x;
			}
			// Y axis constraint exists
			if( child->axes.find( 'y' ) != child->axes.end() )
			{
				newRot.y = dbWrapValue( newRot.y + dy );
				newPos.x += dbNewXValue( oldPos.x, newRot.y, distance ) - oldPos.x;
				newPos.z += dbNewZValue( oldPos.z, newRot.y, distance ) - oldPos.z;
				newPos.y += child->obj->getPosY() - oldPos.y;
			}
			// Z axis constraint exists
			if( child->axes.find( 'z' ) != child->axes.end() )
			{
				newRot.z = dbWrapValue( newRot.z + dz );
				newPos.x += dbNewXValue( oldPos.x, newRot.z, distance ) - oldPos.x;
				newPos.y += dbNewYValue( oldPos.y, newRot.z, distance ) - oldPos.y;
				newPos.z += child->obj->getPosZ() - oldPos.z;
			}

			child->obj->setRotation( newRot );
			child->obj->setPosition( newPos );

			child++;
		}
	}

	this->rotation.x = x;
	this->rotation.y = y;
	this->rotation.z = z;
}


void PoseableObject::constrainToRotation( PoseableObject * child, std::string axes )
{
	list<RotationConstraint>::iterator elem, oldChild = rotationChildren.end();

	for( elem = rotationChildren.begin(); elem != rotationChildren.end(); elem++ )
	{
		if( *( elem->obj ) == *( child ) )
		{
			oldChild = elem;
			break;
		}
	}

	// make sure input string is in uniform case, since the set of axes doesn't contain duplicates
	axes = StringToLower( axes );
	
	// the child was not found in the list, so it had no previous constraints
	if( ( oldChild == rotationChildren.end() ) || rotationChildren.empty() )
	{
		RotationConstraint newChild;
		newChild.obj = child;
		// Insert all listed axes into the set of constrained axes
		for( int i = 0; i < axes.length(); i++ )
		{
			newChild.axes.insert( axes[i] );
		}
		rotationChildren.push_front( newChild );
	}
	else
	{
		for( int i = 0; i < axes.length(); i++ )
		{
			oldChild->axes.insert( axes[i] );
		}
	}

}

// Removes rotation constraints on an object, returning "true" if something was removed,
// and "false" if the specified child object was not found.
bool PoseableObject::removeRotationConstraint( PoseableObject *child, std::string axes )
{
	// make sure input string is in uniform case, so the set of axes doesn't contain duplicates
	axes = StringToLower( axes );

	list<RotationConstraint>::iterator elem, oldChild = rotationChildren.end();

	for( elem = rotationChildren.begin(); elem != rotationChildren.end(); elem++ )
	{
		// Go through the list of children, and if the child is found, remove the
		// specified axes from those being constrained.
		if( *( elem->obj ) == *( child ) )
		{
			oldChild = elem;

			for( int i = 0; i < axes.length(); i++ )
			{
				oldChild->axes.erase( axes[i] );
			}

			// If there are no remaining constraints, remove from the list
			if( oldChild->axes.empty() )
			{
				rotationChildren.erase( elem );
			}

			return true;
		}
	}

	return false;

}
